#include "../include/MapAtlas.hpp"
namespace ORBSLAM
{
    MapAtlas::MapAtlas(/* args */)
    {
        mp_current_map = static_cast<Map*>(NULL);
        mn_last_init_keyframe_id=0;
        Creat_map();
    }

    MapAtlas::~MapAtlas()
    {
    }
    Map *MapAtlas::Get_current_map()
    {
        // mp_current_map = new Map();
        unique_lock<mutex> lock(mmutex_mapatlas);
        if(!mp_current_map){
            Creat_map();
        }
        while(mp_current_map->Is_bad()){
            usleep(3000);
        }
        return mp_current_map;
    }

    void MapAtlas::Add_mappoint(MapPoint *p_mappoint)
    {
    }

    void MapAtlas::Creat_map()
    {
        unique_lock<mutex> lock(mmutex_mapatlas);
        if(mp_current_map){
            if(!msp_map.empty()&&mn_last_init_keyframe_id<mp_current_map->Get_max_keyframeid()){
                mn_last_init_keyframe_id = mp_current_map->Get_max_keyframeid()+1;
            }
            mp_current_map->Set_stored_map();
        }
        mp_current_map = new Map(mn_last_init_keyframe_id);
        mp_current_map->Set_current_map();
        msp_map.insert(mp_current_map);
    }
    
    unsigned long int MapAtlas::Get_last_init_keyframe_id()
    {
        unique_lock<mutex> lock(mmutex_mapatlas);
        return mn_last_init_keyframe_id;
    }

    void MapAtlas::Add_key_frame(KeyFrame *KF)
    {
    }

    vector<KeyFrame *> MapAtlas::Get_all_keyframes()
    {
        return mvp_keyfrmae;
    }
}